#include "GE_Math.h"
#ifdef CHECKMEM
#include "mmgr.h"
#endif
namespace Syp
{
	int Math::nextPowerOf2 (int a )
	{
		int rval=1;
		// rval<<=1 Is A Prettier Way Of Writing rval*=2; 
		while(rval<a) rval<<=1;
		return rval;
	}

	void Math::NormalizePlane(Point4& plane)
	{
		float mag;
		mag = sqrt(plane.x * plane.x + plane.y * plane.y + plane.z * plane.z);
		plane.x = plane.x / mag;
		plane.y = plane.y / mag;
		plane.z = plane.z / mag;
		plane.w = plane.w / mag;
	}

	void Math::getMatrix(AngleAxis& a, float* matrix)
	{		
		float rcos = cos(a.w * PI/180);
		float rsin = sin(a.w * PI/180);
		float u = a.x;
		float v = a.y;
		float w = a.z;

		matrix[0] =      rcos + u*u*(1-rcos);
		matrix[4] =  w * rsin + v*u*(1-rcos);
		matrix[8] = -v * rsin + w*u*(1-rcos);
		matrix[1] = -w * rsin + u*v*(1-rcos);
		matrix[5] =      rcos + v*v*(1-rcos);
		matrix[9] =  u * rsin + w*v*(1-rcos);
		matrix[2] =  v * rsin + u*w*(1-rcos);
		matrix[6] = -u * rsin + v*w*(1-rcos);
		matrix[10] =      rcos + w*w*(1-rcos);
		matrix[3] = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
		matrix[15] = 1.0f;

	}
	
	Quat::Quat()
	{
		w = 1;
		x = 0;
		y = 0;
		z = 0;
	}
	Quat Quat::operator*(const Quat& b)
	{
		//(Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
		//(Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
		//(Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
		//(Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 
		Quat temp;
		temp.w = w * b.w - x * b.x - y * b.y - z * b.z;
		temp.x = w * b.x + x * b.w + y * b.z - z * b.y;
		temp.y = w * b.y - x * b.z + y * b.w + z * b.x;
		temp.z = w * b.z + x * b.y - y * b.x + z * b.w;
		return temp;
	}
	Quat Quat::operator*(const float& b)
	{
		//(Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
		//(Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
		//(Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
		//(Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 
		Quat temp;
		temp.w = w * b;
		temp.x = x * b;
		temp.y = y * b;
		temp.z = z * b;
		return temp;
	}

	Quat Quat::operator +(const Quat& b)
	{
		Quat temp;
		temp.x = x + b.x;
		temp.y = y + b.y;
		temp.z = z + b.z;
		temp.w = w + b.w;
		return temp;
	}

	Quat Quat::convertToQuat()
	{
		axisNormalize();
		//change to radian
		float sin_a = sin( w * (PI/180.0f) / 2 );
		float cos_a = cos( w * (PI/180.0f) / 2 );
		Quat temp;
		temp.x = x * sin_a;
		temp.y = y * sin_a;
		temp.z = z * sin_a;
		temp.w = cos_a;	
		temp.Normalize();
		return temp;

	}

	void Quat::Normalize()
	{
		float L = sqrt(x*x + y*y + z*z + w*w);
		x /= L;
		y /= L;
		z /= L;
		w /= L;
	}

	void Quat::axisNormalize()
	{
		float L = sqrt(x*x + y*y + z*z);
		x /= L;
		y /= L;
		z /= L;
	}
	AngleAxis Quat::convertToAngleAxis()
	{	
		//Real fSqrLength = x*x+y*y+z*z;
		//     if ( fSqrLength > 0.0 )
		//     {
		//         rfAngle = 2.0*Math::ACos(w);
		//         Real fInvLength = Math::InvSqrt(fSqrLength);
		//         rkAxis.x = x*fInvLength;
		//         rkAxis.y = y*fInvLength;
		//         rkAxis.z = z*fInvLength;
		//     }
		//     else
		//     {
		//         // angle is 0 (mod 2*pi), so any axis will do
		//         rfAngle = Radian(0.0);
		//         rkAxis.x = 1.0;
		//         rkAxis.y = 0.0;
		//         rkAxis.z = 0.0;
		//     }
		
		
		
		
		/*AngleAxis a;
		float cos_a = w;
		float angle = acos( cos_a ) * 2;
		float sin_a = sqrt( 1.0 - cos_a * cos_a );
		if ( fabs( sin_a ) < 0.0005 ) 
		{
			sin_a = 1;
		}
		a.x = x / sin_a;
		a.y = y / sin_a;
		a.z = z / sin_a;
		a.w = angle * (180/PI);*/

		float s = x*x + y*y + z*z;
		AngleAxis a;
		if(s > 0.0f)
		{		
			a.w = 2 * acos(w);
			double sL = (1/sqrt(s));
			a.x = static_cast<float>(x * sL);
			a.y = static_cast<float>(y * sL);
			a.z = static_cast<float>(z * sL);
			//Change to degrees ARGHHHHHHHHHHHHHHH wasted 2 days wondering what was wrong
			a.w *= (180/PI); 
		}
		else
		{
			a.w = 0.0f;
			a.x = 1.0f;
			a.y = 0.0f;
			a.z = 0.0f;
		}

		return a;
	}

	Point3 Point3::operator *(const float& b)
	{
		Point3 temp;
		temp.x = x * b;
		temp.y = y * b;
		temp.z = z * b;
		return temp;
	}

	void Point3::Normalize()
	{
		float L = sqrt(x*x + y*y + z*z);
		x /= L;
		y /= L;
		z /= L;	
	}

	bool Point3::operator ==(const Point3& b)
	{
		return(x == b.x && y == b.y && z== b.z);
	}
	void Math::QuatToMatrix(Quat& quat, float *matrix) 
	{
		matrix[0]  = (1.0f - (2.0f * ((quat.y * quat.y) + (quat.z * quat.z))));
		matrix[1]  =         (2.0f * ((quat.x * quat.y) + (quat.z * quat.w)));
		matrix[2]  =         (2.0f * ((quat.x * quat.z) - (quat.y * quat.w)));
		matrix[3]  = 0.0f;
		matrix[4]  =         (2.0f * ((quat.x * quat.y) - (quat.z * quat.w)));
		matrix[5]  = (1.0f - (2.0f * ((quat.x * quat.x) + (quat.z * quat.z))));
		matrix[6]  =         (2.0f * ((quat.y * quat.z) + (quat.x * quat.w)));
		matrix[7]  = 0.0f;
		matrix[8]  =         (2.0f * ((quat.x * quat.z) + (quat.y * quat.w)));
		matrix[9]  =         (2.0f * ((quat.y * quat.z) - (quat.x * quat.w)));
		matrix[10] = (1.0f - (2.0f * ((quat.x * quat.x) + (quat.y * quat.y))));
		matrix[11] = 0.0f;
		matrix[12] = 0.0f;
		matrix[13] = 0.0f;
		matrix[14] = 0.0f;
		matrix[15] = 1.0f;
	}

	void Math::TransposeMatrix(float* mat)
	{
		//This Document                  OpenGL
		//     | 0  1  2  3  |            | 0  4  8  12 |
		//     |             |            |             |
		//     | 4  5  6  7  |            | 1  5  9  13 |
		// M = |             |        M = |             |
		//     | 8  9  10 11 |            | 2  6  10 14 |
		//     |             |            |             |
		//     | 12 13 14 15 |            | 3  7  11 15 |

		float t[16];
		for(int i=0;i<16;++i)
		{
			t[i] = mat[i];
		}
		mat[1] = t[4];
		mat[2] = t[8];
		mat[3] = t[12];
		mat[4] = t[1];
		mat[6] = t[9];
		mat[7] = t[13];
		mat[8] = t[2];
		mat[9] = t[6];
		mat[11] = t[14];
		mat[12] = t[3];
		mat[13] = t[7];
		mat[14] = t[11];
	}


	void Math::MaxToGLMatrix(float* max,float* gl)
	{
		// ============================================================================
		// FixCoordSys(Matrix3 &tm) - Similarity Transformation
		// similarity transformation consists of swapping the 2nd and 3rd rows, then
		// swapping the the 2nd and 3rd columns as above, and THEN negating all
		// the elements of the 1st row, then negating all the elements of the 1st
		// column (with the result that the 11 element is unchanged).
		// ----------------------------------------------------------------------------
		//void FixCoordSys(Matrix3 &tm)
		//{
		//	// swap 2nd and 3rd rows
		//	Point3 row = tm.GetRow(1);
		//	tm.SetRow(1, tm.GetRow(2));
		//	tm.SetRow(2, row);
		//
		//	// swap 2nd and 3rd columns
		//	Point4 column = tm.GetColumn(1);
		//	tm.SetColumn(1, tm.GetColumn(2));
		//	tm.SetColumn(2, column);
		//
		//	// negate 1rst row and column
		//	tm.SetRow(0, -(tm.GetRow(0)));
		//	tm.SetColumn(0, -(tm.GetColumn(0)));
		//}

		//swap 2nd and 3rd row
		//float temp[16];
		//float temp2[16];

		//for(int i=0;i<16;++i)
		//{
		//	temp[i] = max[i];
		//}

		//temp[1] = max[2];
		//temp[5] = max[6];
		//temp[9] = max[10];
		//temp[13] = max[14];

		//temp[2] = max[1];
		//temp[6] = max[5];
		//temp[10] = max[9];
		//temp[14] = max[13];

		////swap 2nd and 3rd columns
		//for(int i=0;i<16;++i)
		//{
		//	temp2[i] = temp[i];
		//}

		//temp2[4] = temp[8];
		//temp2[5] = temp[9];
		//temp2[6] = temp[10];
		//temp2[7] = temp[11];

		//temp2[8] = temp[4];
		//temp2[9] = temp[5];
		//temp2[10] = temp[6];
		//temp2[11] = temp[7];

		////negate 1st row
		//temp2[0] = -temp2[0];
		//temp2[4] = -temp2[4];
		//temp2[8] = -temp2[8];
		//temp2[12] = -temp2[12];

		////negate 1st column
		//temp2[0] = -temp2[0];
		//temp2[1] = -temp2[1];
		//temp2[2] = -temp2[2];
		//temp2[3] = -temp2[3];

		////temp2[3] = temp2[7] = temp2[11] = 0.0f;
		////temp2[15] = 1.0f;

		//1 0 0 0 ;
		//0 0 1 0;
		//0 -1 0 0;
		//0 0 0 1;

		float MaxToGL[16];
		Math::IdentityMatrix(MaxToGL);
		Math::IdentityMatrix(gl);
		MaxToGL[5] = 0.0f;
		MaxToGL[10] = 0.0f;


		MaxToGL[0] = 1.0f;//this is pretty obvious
		MaxToGL[9] = 1.0f;// or is it MaxToGL[9]
		MaxToGL[6] = -1.0f;// or is it MaxToGL[6]??

		Math::multMat(MaxToGL,max,gl);


		//This Document                  OpenGL
		//     | 0  1  2  3  |            | 0  4  8  12 |
		//     |             |            |             |
		//     | 4  5  6  7  |            | 1  5  9  13 |
		// M = |             |        M = |             |
		//     | 8  9  10 11 |            | 2  6  10 14 |
		//     |             |            |             |
		//     | 12 13 14 15 |            | 3  7  11 15 |


	}

	void Math::InverseMatrix(float *mat, float *dst)
	{
		float tmp[12]; /* temp array for pairs */
		float src[16]; /* array of transpose source matrix */
		float det; /* determinant */
		/* transpose matrix */
		for (int i = 0; i < 4; i++) {
			src[i] = mat[i*4];
			src[i + 4] = mat[i*4 + 1];
			src[i + 8] = mat[i*4 + 2];
			src[i + 12] = mat[i*4 + 3];
		}
		/* calculate pairs for first 8 elements (cofactors) */
		tmp[0] = src[10] * src[15];
		tmp[1] = src[11] * src[14];
		tmp[2] = src[9] * src[15];
		tmp[3] = src[11] * src[13];
		tmp[4] = src[9] * src[14];
		tmp[5] = src[10] * src[13];
		tmp[6] = src[8] * src[15];
		tmp[7] = src[11] * src[12];
		tmp[8] = src[8] * src[14];
		tmp[9] = src[10] * src[12];
		tmp[10] = src[8] * src[13];
		tmp[11] = src[9] * src[12];
		/* calculate first 8 elements (cofactors) */
		dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
		dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
		dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
		dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
		dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
		dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
		dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
		dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
		dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
		dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
		dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
		dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
		dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
		dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
		dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
		dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
		/* calculate pairs for second 8 elements (cofactors) */
		tmp[0] = src[2]*src[7];
		tmp[1] = src[3]*src[6];
		tmp[2] = src[1]*src[7];
		tmp[3] = src[3]*src[5];
		tmp[4] = src[1]*src[6];
		tmp[5] = src[2]*src[5];
		tmp[6] = src[0]*src[7];
		tmp[7] = src[3]*src[4];
		tmp[8] = src[0]*src[6];
		tmp[9] = src[2]*src[4];
		tmp[10] = src[0]*src[5];
		tmp[11] = src[1]*src[4];
		/* calculate second 8 elements (cofactors) */
		dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
		dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
		dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
		dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
		dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
		dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
		dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
		dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
		dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
		dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
		dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
		dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
		dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
		dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
		dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
		dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
		/* calculate determinant */
		det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
		/* calculate matrix inverse */
		det = 1/det;
		for (int j = 0; j < 16; j++)
			dst[j] *= det;
	}

	void Math::IdentityMatrix(float* mat)
	{
		for(int i=0;i<16;i++)
		{
			mat[i] = 0.0f;
		}
		mat[0] = mat[5] = mat[10] = mat[15] = 1.0f;//identity matrix
	}

	void Math::multMat(float* A,float* B,float* P)
	{
		//	Let A and B be our matrices,P will be the result (B x A).

		P[0] = A[0] * B[0] + A[4] * B[1] + A[8] * B[2] + A[12] * B[3];
		P[1] = A[1] * B[0] + A[5] * B[1] + A[9] * B[2] + A[13] * B[3];
		P[2] = A[2] * B[0] + A[6] * B[1] + A[10] * B[2] + A[14] * B[3];
		P[3] = A[3] * B[0] + A[7] * B[1] + A[11] * B[2] + A[15] * B[3];
		P[4] = A[0] * B[4] + A[4] * B[5] + A[8] *  B[6] + A[12] * B[7];
		P[5] = A[1] * B[4] + A[5] * B[5] + A[9] * B[6] + A[13] * B[7];
		P[6] = A[2] * B[4] + A[6] * B[5] + A[10] * B[6] + A[14] * B[7];
		P[7] = A[3] * B[4] + A[7] * B[5] + A[11] * B[6] + A[15] * B[7];
		P[8] = A[0] * B[8] + A[4] * B[9] + A[8] * B[10] + A[12] * B[11];
		P[9] = A[1] * B[8] + A[5] * B[9] + A[9] * B[10] + A[13] * B[11];
		P[10] = A[2] * B[8] + A[6] * B[9] + A[10] * B[10] + A[14] * B[11];
		P[11] = A[3] * B[8] + A[7] * B[9] + A[11] * B[10] + A[15] * B[11];
		P[12] = A[0] * B[12] + A[4] * B[13] + A[8] * B[14] + A[12] * B[15];
		P[13] = A[1] * B[12] + A[5] * B[13] + A[9] *B[14] + A[13] * B[15];
		P[14] = A[2] * B[12] + A[6] * B[13] + A[10] * B[14] + A[14] * B[15];
		P[15] = A[3] * B[12] + A[7] * B[13] + A[11] * B[14] + A[15] * B[15];

		//P[12] = 0; //{ Set the bottom row to 0 0 0 1 }
		//P[13] = 0;
		//P[14] = 0;
		//P[15] = 1;

	}

	void Math::InverseQuat(Quat& q)
	{
		q.x = -q.x;
		q.y = -q.y;
		q.z = -q.z;
	}

	void Math::InterpolateQuat(Quat& first,Quat& second,Quat& result,float time)
	{
		//Interpolates between 2 quaternions based on time factor ranging from 0-1
		//Variables
		double cosine;
		Quat temp;	//temp quaternion
		float scale0,scale1;
		bool norm =false;

		//dot product between the 2 quats	
		cosine = first.x * second.x + first.y * second.y + first.z * second.z + first.w * second.w;


		if(cosine<0.0)
		{
			//if it is negative, make it positive
			cosine = -cosine;
			temp.x = -second.x;
			temp.y = -second.y;
			temp.z = -second.z;
			temp.w = -second.w;
			norm = true;
		}
		else
		{
			temp.x = second.x;
			temp.y = second.y;
			temp.z = second.z;
			temp.w = second.w;
		}

		if ( (1.0 - cosine) > 0.0001 ) {
			double omega, sinom;
			// standard case (slerp)
			omega = acos(cosine);
			sinom = sin(omega);
			scale0 = static_cast<float>(sin((1.0 - time) * omega) / sinom);
			scale1 = static_cast<float>(sin(time * omega) / sinom);
		}
		else {//else do LERP
			scale0 = 1.0f - time;
			scale1 = time;		
		}
		if(norm)
		{
			temp.Normalize();
		}


		//result.x = first.x * scale0 + temp.x * scale1;
		//result.y = first.y * scale0 + temp.y * scale1;
		//result.z = first.z * scale0 + temp.z * scale1;
		//result.w = first.w * scale0 + temp.w * scale1;
		//Test this later on, looks cleaner
		result = (first * scale0) + (temp * scale1);

	}

	Point3 Point3::operator +(const Point3& b)
	{
		Point3 temp;
		temp.x = x + b.x;
		temp.y = y + b.y;
		temp.z = z + b.z;
		return temp;
	}
	Point3 Point3::operator -(const Point3& b)
	{
		Point3 temp;
		temp.x = x - b.x;
		temp.y = y - b.y;
		temp.z = z - b.z;
		return temp;
	}


	void Math::InterpolatePoint3(Point3& first, Point3& second, Point3& result, float time)
	{
		result = first + ((second - first) * time);

	}

	void Math::MultVectorByMatrix(float* m, float* v,float weight, float* result)
	{
		result[0] += ((m[0] * v[0]) +
			(m[4] * v[1]) +	
			(m[8] * v[2]) +
			m[12]) * weight;
		result[1] += ((m[1] * v[0]) +
			(m[5] * v[1]) +	
			(m[9] * v[2]) +
			m[13]) * weight;
		result[2] += ((m[2] * v[0]) +
			(m[6] * v[1]) +	
			(m[10] * v[2]) +
			m[14]) * weight;
	}

	void Math::MultNormalByMatrix(float* m, float* v, float weight,float* result)
	{
		result[0] += ((m[0] * v[0]) +
			(m[4] * v[1]) +	
			(m[8] * v[2])) * weight;
		result[1] += ((m[1] * v[0]) +
			(m[5] * v[1]) +	
			(m[9] * v[2])) * weight;
		result[2] += ((m[2] * v[0]) +
			(m[6] * v[1]) +	
			(m[10] * v[2])) * weight;
	}
}
